/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.domain;

import br.ufrn.engcomp.mpc.Constants;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.RealMatrix;
import org.apache.commons.math.linear.RealVector;

/**
 *
 * @author Felipe
 */
public class CoupledTanksPlantModelWithConstraints extends CoupledTanksPlantModel implements PlantWithConstraints {

    @Override
    public RealMatrix getStaticEConstraintsMatrixSeed() {
        return null;
    }

    @Override
    public RealMatrix getStaticFConstraintsMatrixSeed() {
        RealMatrix fConstraintsMatrixSeed = new Array2DRowRealMatrix(2, Constants.L + 1);
        double[] lowerLimit = computeLowerLimitValue(inputOP[0], -LIMITE_ATUADOR);
        double[] upperLimit = computeUpperLimitValue(inputOP[0], LIMITE_ATUADOR);
        
        // -3 <= u <= 3
        fConstraintsMatrixSeed.setEntry(0, 0, lowerLimit[0]);
        fConstraintsMatrixSeed.setEntry(0, 1, lowerLimit[1]);
        fConstraintsMatrixSeed.setEntry(1, 0, upperLimit[0]);
        fConstraintsMatrixSeed.setEntry(1, 1, upperLimit[1]);
        
        return fConstraintsMatrixSeed;
    }

    @Override
    public RealMatrix getStaticGConstraintsMatrixSeed() {
        RealMatrix gConstraintsMatrixSeed = new Array2DRowRealMatrix(2, Constants.N + 1);
        double[] lowerLimit = computeLowerLimitValue(stateOP[0], LIMITE_INFERIOR_TANQUES);
        double[] upperLimit = computeUpperLimitValue(stateOP[0], LIMITE_SUPERIOR_TANQUES);
        
        // 3 <= L1 <= 27
        // 3 <= L2 <= 27
//        gConstraintsMatrixSeed.setEntry(0, 0, lowerLimit[0]);
//        gConstraintsMatrixSeed.setEntry(0, 1, 0);
//        gConstraintsMatrixSeed.setEntry(0, 2, lowerLimit[1]);
//        gConstraintsMatrixSeed.setEntry(1, 0, upperLimit[0]);
//        gConstraintsMatrixSeed.setEntry(1, 1, 0);
//        gConstraintsMatrixSeed.setEntry(1, 2, upperLimit[1]);
//        gConstraintsMatrixSeed.setEntry(2, 0, 0);
//        gConstraintsMatrixSeed.setEntry(2, 1, lowerLimit[0]);
//        gConstraintsMatrixSeed.setEntry(2, 2, lowerLimit[1]);
//        gConstraintsMatrixSeed.setEntry(3, 0, 0);
//        gConstraintsMatrixSeed.setEntry(3, 1, upperLimit[0]);
//        gConstraintsMatrixSeed.setEntry(3, 2, upperLimit[1]);
        
        gConstraintsMatrixSeed.setEntry(0, 0, upperLimit[0]);
        gConstraintsMatrixSeed.setEntry(0, 1, 0);
        gConstraintsMatrixSeed.setEntry(0, 2, upperLimit[1]);
        gConstraintsMatrixSeed.setEntry(1, 0, 0);
        gConstraintsMatrixSeed.setEntry(1, 1, upperLimit[0]);
        gConstraintsMatrixSeed.setEntry(1, 2, upperLimit[1]);
        
        return gConstraintsMatrixSeed;
    }

    @Override
    public RealMatrix getDynamicEConstraintsMatrixSeed() {
        return null;
    }

    @Override
    public RealMatrix getDynamicFConstraintsMatrixSeed(RealVector lastXVector) {
        RealMatrix fConstraintsMatrixSeed = null;
        double[] lowerLimit = computeLowerLimitValue(inputOP[0], -LIMITE_ATUADOR);
        double[] zeroUpperLimit = computeUpperLimitValue(inputOP[0], 0);
        double[] zeroLowerLimit = computeLowerLimitValue(inputOP[0], 0);
        double[] upperLimit = computeUpperLimitValue(inputOP[0], LIMITE_ATUADOR);
        
        // Se o tanque #1 estiver abaixo de 3cm, então 0 <= u <= 3
        if (lastXVector.getEntry(0) < LIMITE_INFERIOR_TANQUES) {
            fConstraintsMatrixSeed = new Array2DRowRealMatrix(2, Constants.L + 1);
                    
            fConstraintsMatrixSeed.setEntry(0, 0, zeroLowerLimit[0]);
            fConstraintsMatrixSeed.setEntry(0, 1, zeroLowerLimit[1]);
            fConstraintsMatrixSeed.setEntry(1, 0, upperLimit[0]);
            fConstraintsMatrixSeed.setEntry(1, 1, upperLimit[1]);
        // Se o tanque #1 estiver acima de 27cm, então -3 <= u <= 0
        } 
//        else if (lastXVector.getEntry(0) > LIMITE_SUPERIOR_TANQUES) {
//            fConstraintsMatrixSeed = new Array2DRowRealMatrix(2, Constants.L + 1);
//            
//            fConstraintsMatrixSeed.setEntry(0, 0, lowerLimit[0]);
//            fConstraintsMatrixSeed.setEntry(0, 1, lowerLimit[1]);
//            fConstraintsMatrixSeed.setEntry(1, 0, zeroUpperLimit[0]);
//            fConstraintsMatrixSeed.setEntry(1, 1, zeroUpperLimit[1]);
//        }
        
        return fConstraintsMatrixSeed;
    }

    @Override
    public RealMatrix getDynamicGConstraintsMatrixSeed(RealVector lastStepVector, RealVector setPointVector) {
        RealMatrix gConstraintsMatrixSeed = null;
        double lastStep = lastStepVector.getEntry(0);
//        double[] lowerLimit = computeLowerLimitValue(stateOP[1], (stateOP[1] + setPointVector.getEntry(0)) - (Math.abs(lastStep) * 0.30));
//        double[] upperLimit = computeUpperLimitValue(stateOP[1], (stateOP[1] + setPointVector.getEntry(0)) + (Math.abs(lastStep) * 0.30));
        double[] lowerLimit = computeLowerLimitValue(stateOP[1], (stateOP[1] + setPointVector.getEntry(0)) * 0.95);
        double[] upperLimit = computeUpperLimitValue(stateOP[1], (stateOP[1] + setPointVector.getEntry(0)) * 1.05);
        
        // Restrição de overshoot = 0% para set-point acima, logo L2 <= setPoint
        if (lastStep > 0) {
            gConstraintsMatrixSeed = new Array2DRowRealMatrix(1, Constants.N + 1);
            
            gConstraintsMatrixSeed.setEntry(0, 0, 0);
            gConstraintsMatrixSeed.setEntry(0, 1, upperLimit[0]);
            gConstraintsMatrixSeed.setEntry(0, 2, upperLimit[1]);
        // Restrição de overshoot = 0% para set-point abaixo, logo L2 >= setPoint
        } else if (lastStep < 0) {
            gConstraintsMatrixSeed = new Array2DRowRealMatrix(1, Constants.N + 1);
            
            gConstraintsMatrixSeed.setEntry(0, 0, 0);
            gConstraintsMatrixSeed.setEntry(0, 1, lowerLimit[0]);
            gConstraintsMatrixSeed.setEntry(0, 2, lowerLimit[1]);
        }
        
        return gConstraintsMatrixSeed;
    }

    @Override
    public String getCodeName() {
        return "LPWC";
    }
    
}
